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ABSTRACT 

This  report  presents  a  convex  rel  axati  on  method  that  gl  obal  I  y  sol  ves  for  the  camera  posi  ti  on  and 
ori  entati  on  from  a  set  of  i  mage  pi  xel  measu  rements  associ  ated  w  i  th  a  scene  of  reference  poi  nts  of 
known  3D  positions.  The  pose  optimisation  is  formulated  as  a  semi  definite  positive  relaxation 
program  and  this  approach  shows  superior  performance  over  existing  methods. 
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Optimal  3D  Localisation  from  I  mage  Pixel  Measurements 


Executi  ve  Su  mmary 


Optical  sensors,  such  as  digital  cameras,  use  pixel  measurements  to  estimate  the  object 
viewing  direction  with  high  accuracy.  They  are,  therefore,  prime  candidates  in  defence 
appl  i  cati  ons  requi  ri  ng  preci  se  I  ocal  i  sati  on  and  ori  entati  on  determi  nati  on .  I  n  thi  s  report  we 
present  a  convex  relaxation  method  that  globally  solves  for  the  camera  position  and 
orientation  given  a  set  of  image  pixel  measurements  associated  with  a  scene  of  reference 
points  of  known  3D  positi  ons.  Theapproach  formulates  theposeoptimizati  on  problem  as 
a  semidefinite  positive  relaxation  (SDR)  program.  A  comprehensive  comparative 
performance  analysis,  carried  out  in  the  computer  simulations  section,  demonstrates  the 
superior  performance  of  our  relaxation  method  over  existing  approaches.  The 
computational  complexity  of  the  method  is  0(n),  where  n  is  the  number  of  reference 
points. 
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1.  Introduction 

The  w  i  d  esp  read  use  of  cal  i  brated  cameras  stems  from  their  ability  to  measure  d  i  recti  on  or  angl  e 
(intheformof  pixels)  with  high  accuracy.  This  explains  why  they  are  often  primecandi  dates  in 
applications  involving  precise  localisation  and  orientation  determination,  particularly  in 
robotics,  augmented  reality  and  defence (Campaetal.  2006). 

Pose  estimation  is  also  known  i  n  the  literature  as  the  Perspective-n-Point  problem  (PnP),  where 
the  objective  is  to  estimate  the  camera  pose  based  on  image  measurements  of  known  reference 
points  (H  artley  and  Zisserman  2003).  The  reference  points  are  sometimes  called  landmarks, 
control  points  or  markers  and  their  positions  are  assumed  to  be  given  in  a  world  coordinate 
frame. 

The  literature  on  pose  estimation  is  extensive  and  includes  minimal  approaches,  which  are 
restricted  to  three  landmarks  (Haralick  dial.  1991),  and  moregen  eric  approaches  that  handle  more 
reference  points  (Ansar  and  Daniilidis  2003;  Lepetit  et  al.  2008;  Lu  el  at.  2000;  Quan  and  Lan 
1999;  Schweighofer  and  Pinz  2006). 

Among  the  latter  approaches,  some  employ  iterativetechniques(Lu  el  at.  2000;  Schweighofer 
and  Pinz  2006),  while  others  build  a  large  polynomial  system  and  apply  linear  algebra 
techniques  (Ansar  and  Daniilidis2003;Quan  and  Lan  1999)tosolveforthedistanceinformation 
first,  then  the  camera  position  and  rotation.Theposecomputational  complexity  varies  between 
0(n2)  (Quan  and  Lan  1999)  and  0{n  )  (Ansar  and  Daniilidis  2003). 

Recently  a  third  group  of  efficient  approaches  (Lepetit  etal.  2008;  Schweighofer  and  Pinz  2008) 
emerged  that  are  only  0(n )  and  yet  display  improved  performance  over  many  existing  pose 
estimation  methods.  These  will  be  discussed  in  some  detail  in  the  remaining  part  of  this 
introduction,  butonecould  also  refer  to  (Lepetit  dial.  2008)  and  the  references  therein  for  more 
details  on  existing  pose  approaches. 

The  orthogonal  iterative  (Ol)  approach  of  (Lu  el  at.  2000)  is  cited  here  because  of  its  high 
estimation  accuracy  and  relatively  fast  convergence  speed  provided  that  the  algorithm  is 
properly  i  niti  al  ised .  Copl  anar  confi  gurati  on  poi  nts  or  reference  poi  nts  projecti  ng  onto  one  si  de 
of  the  image  are  known  to  cause  the  O I  algorithm  to  perform  poorly  (Schweighofer  and  Pinz 
2006;  Lepetit  etal.  2008).  A  rigorous  analysis  on  the  effect  of  copl  anar  point  scenes  is  given  in 
(Schweighofer  and  Pinz  2006),  which  demonstrates  the  existence  of  two  ambiguous  solutions 
and  proposes  a  modified  algorithm  that  rectifies  the  behaviour  of  the  Ol  algorithm  when 
presented  with  planar  scenes. 

Th  eEfficientPnP  (EPnP)  algorithm  developed  in  (Lepetit  etal.  2008)  uses  the  concept  of  virtual 
control  points  (VCP)  to  solve  for  the  camera  pose.  This  concept  is  novel  as  it  does  not  directly 
search  for  the  rotation  and  translation  of  the  rigidity  transformation.  It  instead  solves  for  the 
local  camera  coordinates  of  four  points  whose  positions  are  only  given  in  a  world  reference 
frame.  To  achieve  this,  a  constrained  quadratic  optimisation  problem  (having  6  quadratic 
equality  constraints)  is  developed  and  then  approximately  solved  by  performing  a  four-case 
null  space  analysis  of  a  12  x  12  objective  fundi  on  matrix.  Once  the  local  VCP  coordinates  are 
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esti  mated  the ri  gi d  transformati  on  i  s  determi  ned  usi  ng  theabsol  uteori  entati on  al gorithm  (H  orn 
dial.  1988). 

Thecomplexity  of  this  algorithm  is  0(n)  and  anumberof  performanceplotswereindudedto 
demonstrate  i  mproved  effi  ci  ency,  parti  cul  arly  i  n  terms  of  processi  ng  speed .  The  pose  esti  mati  on 
accuracy,  however,  remains  lower  than  that  based  on  minimisingtheobjectspaceerror(referto 
(Lu  el  at.  2000)  or  (Schweighofer  and  Pinz  2008)).  It  remains  unclear  how  much  the  proposed 
EPnP  solving  method,  which  is  based  on  linear  al  gebra  techniques,  contributes  to  theobserved 
performance. 

The  more  recent  work  of  (Schweighofer  and  Pinz  2008)  proposes  a  sum-of-squares  (SOS) 
relaxation  approach  to  find  the  globally  optimal  solution  to  thecost  fundi  on  identical  to  that  of 
(Lu  el  at.  2000).  Its  performance  accuracy  is  a  significant  improvement  over  those  of  many 
previ ous  methods.  Its  mai  n  I  i  mitati on  i s  that  it  makes  a  clear  di sti  ndi on  between  coplanar  and 
non-coplanar  point  configurations  and  therefore  executes  two  variants  of  the  SOS  relaxation 
algorithm  before  deciding  on  the  globally  optimal  solution.  Execution  times  are  much  slower 
than  those  of  (Lepetit  et  al.  2008)  but  are  orders  of  magnitude  faster  than  branch-and-bound 
based  pose  estimators  (Hartley  and  Kahl  2009;  Olsson  etal.  2006;  Olsson  etal.  2008). 

In  this  report  we  present  a  novel  semi  definite  program  (SDP)  that  globally  solves  for  the  camera 
pose.  The  cost  fundi  on  to  be  mi  ni  mi  sed  i  s  based  on  the  accumul  ated  objed  space  error  as  gi  ven 
in  (Lu  el  at.  2000;  Schweighofer  and  Pinz  2008).  However,  we  impose  an  additional  chirality 
constraint  which  ensures  that  theobtai  ned  optimal  poseisalsorealisticinthesensethatviewed 
reference  poi  nts  (or  at  I  east  the  majority  of  them)  I  i  e  i  n  front  of  the  camera.  The  computational 
complexity  of  our  pose  esti  mati  on  method  is  0(n) . 

Furthermore,  this  approach  does  away  with  thedistindi on  between  coplanar  and  non-coplanar 
point  configurations,  and  provides  a  unified  formulation  for  both  configurations.  The  average 
recorded  runtime  is  only  about  50  ms  for  a  point  count  of  100,  which  represents  a  significant 
runtimeredudionfor  an  algorithm  that  performs  global  search  for  the  optimal  pose. 

This  report  builds  on  the  work  of  (Kim  and  H  mam  2009)  and  provides  a  global  solution  for  the 
pose  probl  em.  Thi  s  report  i  s  subd  i  vi  ded  as  fol  I  ows:  Sedi  on  2  gi  ves  a  formal  presentati  on  of  the 
pose  problem  to  be  solved.  Sedion  3  presents  the  proposed  solving  method  based  on  SDP 
relaxation.  A  detailed  performanceanalysisofthemethod, as  applied  to  syntheticand  real  data, 
i  s  carri  ed  out  i  n  Sedi  on  4.  Sedi  on  5  provi  des  cond  udi  ng  remarks  and  future  research  di  redions. 
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2.  Problem  Formulation 


Given  a  set  of  correspondences  between  n  reference  points,  p, ,  i=l...n  in  3D  space  and  their 
associated  i  mages,  the  objective  is  to  estimatethe  rotation  and  position  of  the  calibrated  camera. 
I  n  thi  s  work  we  achieve  this  estimation  through  mini  mi  sing  the  alignment  error  sum  between 
the  measured  and  actual  pointing  vectors  as  shown  below. 


minimise  >  w, 

R  t  ' 

’  1=1 


V; 


(Rp,  +t) 


subject  to  R  e  SO( 3) 


(1) 


where  v,  isthe3D  vector  pointing  along  the  im  measured  image  pixel  and  whose  origin  isthe 
optical  centre,  SO(3 )  is  the  set  of  3D  rotations,  R  is  a  given  rotation  from  SO( 3) ,  and  t 
represents  a  3D  translation  vector.  The  parameters,  w,  >  0 ,  are  measurement  weights.  I  n  this 
study,  they  are  all  set  to  lfor  simplicity's  sake. 

Note  that  the  objective  fundi  on  (1)  can  also  be  expressed  as 

2 

(2) 


n 

(  T  \ 

E{  R,t)  =  Xw-' 

i=l 

1  i  ' 

T 

\  vi  vi ) 

(Rp,  +t) 

which  appears  in  (Lu  etal.  2000;  Schweighofer  and  Pinz  2006;  Schweighofer  and  Pi nz  2008).  For 
a  proof  of  thi  s  refer  to  A  ppendi x  A . 

A  close  examination  of  theobjedivefundion  clearly  reveals  that  (1)  is  minimised  when  all  or 
most  of  the  cross  produd  vedors,  —  V(Rp,+t),  are  smal  I .  Thi  s  occurs  if  most  of  the  vedors, 

Nl 

Rp,.  + 1 ,  point  approximately  along  thediredion  y.  or  in  the  opposite  diredion  (i.e.,  -  v. ). 
Thi  s  second  al ternati  ve  i s  not  acceptabl  e  as  it  i  mpl  i  es  that  the  reference  poi  nts  are  behi  nd  the 

n 

camera.  To  enforce  thi  s  observati  on,  weadd  thechi  rality  constrai  nt,  ^  vf  (Rp,  + 1)  >  0 ,  and  the 

i=i 

optimisation  problem  then  becomes 


minimise  £(R,t)  =  ^w(.  ||  (i  -  V,-)(Rp,  +t)f 

K,t  i=i 

n 

subject  to  Ivf(Rp,+t)>0 

i=i 

Re  50(3) 


(3) 


where  V,  = 


vv 


T 

V;  y. 
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From  (3)  the  optimal  translation  vector,  t  ,  is  obtained  by  setting  Vt£(R,t)  to  zero. 


v  tE  (R,  t)  =  X  A  {2(1  -  V,  )t  +  2(1  -  V,  )Rp , }  =0  (4) 

i= 1 


Solving  (4)  leads  to 


n 


t0,((R)  =  -A^ZA-RP<- 

i= 1 


(5) 


n 

where  A  =  £  A,-  with  A;  =  w,. 
i=l 


(I-V,). 


The  expression  for  t0/),  in  equation  (5)  contai  ns  the  unknown  term  R ,  which  needstobesolved. 
This  is  done  by  expressing  R  as  a  function  of  the  coordinates  of  a  unit  quaternion  (i.e., 

q  =  [?l.?2.?3»?4])- 


R  = 


2  2  2  2 
9  1 2~' 3  ^  4 

2(^3  ~^4) 
2(q2q4  +  qxq3) 


2(q2q2+qiqA) 

q\-q22+q]-ql 
^(q3qA  -qxq2) 


2{q2q4 -qxq3) 

2(q2q4  +qlq2) 

2  2  2  2 

q  \—q i~q i+q 4 


where  q]+q22+q23+q24=  1 . 


If  wedefine 


e  =  [q],  qxq2,  qxq3,  qxq4,  q\,  q2q3,  q2q4,  q\,  q3q4,  q24]T  (6) 

then  the  product  term,  Rp  in  (5),  can  be  rearranged  as 

RPi  =  Q,e  (7) 

where 


1 

As 

S' 

0 

-*P* 

2  Piy 

Pi, 

2P  iy 

2  Piz 

-Pi, 

>3 

cC 

1 

o 

p 

II 

Piy 

2Pi, 

0 

~  2  Pa 

- Piy 

2Pi, 

0 

Piy 

2Pi,  ~Piy 

Pi, 

~  2P  iy 

2/4 

0 

~Pi, 

0 

2  Pix 

-Pi, 

2  Piy  Pi, 

and  P,  =  [Pu,Pfy,Pb]T  ■ 


This  alternative  expression  Q,e  allows  the  unknowns  in  R  tobefactored  out  in  the  form  of 
linear  vectors. 
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n 

If  we  let  F  =  -A “^A.Q,.  and  G,  =  F  +  Q(  then  it  is  straightforward  to  show  that 


i= 1 


topi(R)  =  Fe 

RP,  +  topt  =  G,e  ■ 

n 

Substituting  (9)  into  the  chirality  constraint  (Rp,  +topt)  in  (3)  leadsto 

i=l 

2>f(  RP,+<o„)  =  hre 

1  =  1 

n 

where  h  =  ^Gf y(. . 


1=1 


(8) 

(9) 


(10) 


Similarly,  theobjectivefunction  reduces  to  F(e)  =  erMe ,  where  M  =  ^Gf  A,G,  is  a  10x10 

i=l 

semidefinite  positive  matrix.  Thismatrix  Mean  be  computed  from  the  known  quantities,  p 
and  v, .  Problem  (3)  then  becomes 

minimise  erMe 

e 

subject  to  h  e  >  0 
kre  =  1 

e  =  [q],  qxq2,  qxq3,  qxq4,  q\,  q2q3,  q2q4,  q\,  q3q4,  q24Y 

Thevector  k  =  [1,0,  0,  0, 1,  0,  0, 1,  0,  l]r  ensuresthat  e  satisfiesthequaternion normalisation 
property,  q]+q22+q]+q24=  ex  +e5  +e8  +e10  =  1 .  Notethatbeforesolving(ll)itisgood practiceto 
normalise  M  and  h  to  improve  numerical  stability. 
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3.  Semi  definite  Programming  Relaxation 


Inthissection  weapply  semi  definite  programming  theory  (Boyd  and  Vandenberghe2004)to 
approximate  the  solution  of  (10).  The  objective  function  in  (11)  may  be  expressed  as 
e  T  Me  =  7r(Mee  r )  =  TV  (MX)  whereX  =  eer  isrank  lpositivesemidefiniteand  TrQ  isthetrace 
operator  (i.e.,  sum  of  diagonal  elements).  It  can  be  shown  that  20  linear  relationships  can  be 
established  among  the  entries  ofX.  These  relationships  originate  from  the  algebraic 
interdependencies  that  govern  the  quartic  monomials  of  X .  For  example,  multiplying  thefirst 
and  fifth  elements  of  e  ,  results  in  e1e5  =  q\q\=  (qlq2)2  =e\  .This  then  translates  into  the  I  inear 
equality  constraint,  X15  -X22  =0  .Anexhaustivelistof20linearcomponentdependenciesof  X 
is  given  in  Appendix  B. 

The  constraint  functions  in  (11)  only  include  quadratic  monomials.  These  constraints  can  be 
captured  by  introducing  another  rank  1  semidefinite  positive  matrix,  Y  =  qq '  ,  where 
q=[ql,q2,q3,q4f .  Note  that,  as  shown  in  Appendix  B,  the  trace  of  Y  is 
q]+q22+q]+q24=  ke  =  1 .  Furthermore,  becauseany  arbitrary  entry  of  e  can  also  bewritten  as 
e.  =  e j  (q]+q22+ q]+ q24) ,  then  lOadditional  constrai  nts  may  begenerated  by  relating  entries  of  X 
to  those  of  Y.  For  example,  given  that  e7=q2q4(=  Y24),  then  we  have 

q2q4  =  <7i2<72<74  +qlq4  +qiq2q4  +qiql>  resulting  in  the  linear  equality  constraint 
X17  +X57  +X87  +X10  7  -  Y24  =  0  .AppendixBlistsall  10 1  inear  relationships  that  constrain  X 
and  Y . 


N  ext  we  combi  ne  X  and  Yintoa  14x14  block-diagonal  matrix 


X 

0 

— 

0 

Y 

.  Because  both 


matrices  are  positivesemidefinite,  itfollowsthat  Z  isalso  positivesemidefinite.  Problem  (11) 
can  then  be  relaxed  to  become 


minimise  7>(MZ) 
z 

subject  to  7r(HZ)  >  0 

7>(K,Z)  =  bl ,  1  <  z  <  3 1 
Z>0 


(12) 


M 

O' 

0 

0" 

where  M  = 

0 

0 

and  H  = 

0 

H 

Thematrices,  K,.,  1  <  z  <  3 1,  enforcethe  31  equality  constrai  nts  described  above  and  given  in 

Appendix  B.  The  symmetric  matrix,  H ,  is  a  matrix  representation  of  the  vector,  h ,  and  is 
defined  such  that  hre  =  7V(HY) .  FI  ence 
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hx  h2  h3  h4 

0  h5  h6  h7 

0  0  hs  hg 

0  0  0  h10 


03) 


P  robl  em  ( 12)  i  s  an  SD  P  rel  axati  on  of  (11)  as  the  ran  kl  constraint  on  both  X  and  Y  isdropped. 
If  the  ranks  of  the  solutions  X  and  Y  are  both  1,  then  the  global  solution  of  the  relaxation 
problem  (12)  corresponds  to  the  global  solution  of  (11).  If  the  solutions  X  and  Y  are  not  of 
rank  1,  then  solving  (12)  only  provides  a  lower  bound  for  the  cost  function,  erMe ,  in  (11). 

It  is  well  known  that  SDP  problems  such  as  (12)  can  be  solved  globally  in  polynomial  time  using 
efficient  algorithms  based  on  interior  point  methods  (Boyd  and  Vandenberghe2004).  A  number 
of  software  packages  have  been  developed  and  released  for  public  or  commercial  use 
(e.g„  SeDuM  i  (Sturm  1999)).  Using  SeDuM  i,  for  example,  the  constraints  of  (12)  arecaptured  in 
a  relatively  small  matrix,  A ,  of  size  118x32  .This  size  remains  fixed  regardless  of  the  number 
of  reference  points,  n  .  M  ore  details  on  how  to  run  SeDuM  i  and  how  to  formulate  the  input 
arguments  aregiven  in  Appendix  C. 
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4.  Synthetic  and  Real  Image  Results 

Beforedisplayingthe  pose  estimation  performance  plots,  weshould  point  out  that  we  have  also 
implemented  a  fourth  degree  SOS  relaxation  algorithmtosolve(lO).  Appendix  D  provides  the 
details  of  the  SOS  relaxation  formulation,  which  bears  some  similarities  with  the  published 
algorithm  in  (Schweighofer  and  Pinz  2008),  exceptfortheadditional  chirality  condition.  After 
executing  a  large  number  of  SOSbased  and  SDR  pose  simulations,  we  observed  almost 
identical  performances  in  terms  of  estimation  accuracy.  However,  our  SDR  algorithm  runs 
about  twice  as  fast  (because  of  the  much  smaller  matrix  A ). 

I  n  this  paper  we  consider  a  comparative  performance  analysis  with 

•  the  orthogonal  iterative(OI)  algorithm  of  (Lu  etal.  2000),  denoted  as  LH  M, 

•  the  modified  Ol  algorithm  of  (Schweighofer  and  Pinz  2006)  denoted  as  M  LH  M , 

•  the  EPnP  algorithm  of  (Lepetit  el  al.  2008),  denoted  as  EPnP, 

•  the  proposed  SD P  rel axati on  based  al gorithm,  denoted  as  SD R,  and 

TheSDR  algorithm  isexecuted  by  callingthesedum/  command  (seeAppendixC)oftheSeDuMi 
software  package  (Sturm  1999).  The  Sedumi  accuracy  parameter  is  maintained  at  10"8 
Accuracies  beyond  10"8  only  result  in  numerical  convergence  problems  and  delays  with  no 
significant  improvement  to  thesoluti  on  accuracy.  A  local  search  is  optionally  applied  to  refine 
the  obtained  SDR  solution  at  a  negligible  processing  cost.  This  search  is  implemented  as  an 
unconstrained  conjugate  gradient  (CG)  descent  applied  to  the  cost  function  of  (12)  written  in 
terms  of  Euler  angles. 

The  issue  of  SDP  relaxation  tightness  has  been  closely  investigated  numerically.  Relaxation 
tightness  is  indicated  by  a  rank  1  solution.  H  owever,  numerous  computer  simulations  showed 
decreased  relaxation  tightness  associated  with  poorer  camera/  point  geometries  because  the 
rank  1  condition  was  frequently  not  met  with  strict  tolerance.  This  meant  that  correction 
measures  were  needed  to  i  ncr  ease  the  rel  axati  on  ti  ghtness  w  hi  I  e  preservi  ng  convexity.  Thi  s  was 
achieved  by  perturbing  (12)  according  to 

min  7r(MZ)  -  a  7>(HZ) 
z 

subject  to  7V( HZ)  >  s 

Tr(KiZ)  =  bl,  1  <  i  <  3 1 
Z>0 

It  was  found  that  setting  a  ~  0.0002  andf  «  0.2  ,  yields  significant  performance 
improvement.  The  main  goal  ofthisnew  formulation  is  to  generate  an  optimal  solution  that  not 
only  reduces  7>-(MZ)  but  also  increases  (HZ)  beyond^ .  Using  this  new  perturbed  formulation 
the  SDP  relaxation  became  significantly  tighter. 

I  n  al  I  computer  si  mul  ati  ons  carri  ed  out  i  n  thi  s  paper,  we  use  a  vi  rtual  perspecti  vecamera  model 
having  a  focal  length  of  f  =  800  and  a  principal  poi  nt  at  (uc,vc)  =  (320,240)  .The  3D  reference 
points  are  selected  using  a  uniform  distribution  within  the  field  of  view,  and  any  point  that 
projects  outside  the  640x480  image  plane  is  discarded  from  further  consideration.  Each 
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projected  image  pixel  is  then  subjected  to  additive  Gaussian  noise  of  varying  standard 
deviations  and  then  quantised  by  rounding  its  coordinate  to  the  nearest  integer. 

Wenext  define  the  rotation  and  position  error  metrics  as 

pos  _  err  =  \\ptrue  -pj  -  -rLAw|| 

rot  _  err  =  2  cos-1  ( q0 ) , 
where  q0  =  0.5^1  +  EL1  +  E2  2  +  E33 

and  Eu,E22  and  E3  3  are  diagonal  elements  of  E  =  Res/R^.1(e. 

Fi  nal  I  y,  we  shoul  d  i  nd  i  cate  that  al  I  parameters  control  ling  the  SD  R  al  gorithm  are  mai  ntai  ned 
fixed  according  to  pose  formulation  (14).  The  associated  A-matrix  in  the  SeDuMi 
implementation  remains  unaffected  by  theintroduced  correction  terms  in  (14). 

Also,  unless  otherwise  mentioned,  the  SDR  algorithm  is  assumed  to  include  the  fine-tuning 
step.  The  Matlab  programs  for  the  M  LH  M  and  EPnP  are  obtained  from  their  respective  web 
sources1. The EPnP  algorithm  is  always  executed  with  theGauss-Newton  fine-tuning  enabled. 

4.1  Pose  Estimation  Error  versus  I  mage  Pixel  Noise 

Figure  1  analyses  the  performance  accuracy  as  a  function  of  i  mage  pixel  errors.  The  reference 
points  are  selected  within  the  camera  field  of  view,  insidethe3D  box[-2, 2]x[-2, 2]x[4, 8] . 
Fifteen  pixel  error  increments  were  implemented  in  the  computer  simulations. 

The  four  subplots  in  Figure  1  depict  the  root  mean  square  (RMS)  and  median  errors  of  the 
position  and  rotation  estimates  for  a  6-landmark  configuration  selected  uniformly  within  the 
box.  As  i  s  cl  ear  from  the  pi  ots,  the  SD  R-based  positi  on  and  rotati  on  esti  mati  on  accuraci  es  are 
higher  than  those  of  LFIM  and  EPnP.  The  SDR  algorithm  foil  owed  by  local  search,  labelled  as 
SDR-fCG  in  thefigure,  display  relatively  higher  position  estimation  accuracy,  particularly  for 
larger  pixel  errors. 

The  purpose  of  including  the  median  estimation  error  alongsidethe  root  mean  square  error 
(RMSE)  isto  mitigate  the  effect  of  outliersdueto  algorithm  convergence  to  local  minima.  Figure 
1  shows  comparatively  similar  RMS  and  median  error  pi  ots  for  the  SDP  based  algorithms  but 
not  for  the  remai  ni  ng  two.  Thi  s  i  s  an  i  nd  i  cati  on  that  si  gnifi  cantl  y  more  outl  i  ers  aregenerated  by 
the  EPnP  and  LFH  M  pose  approaches. 

Figure  2  is  added  to  closely  examine  the  convergence  behaviour  of  the  LFH  M,  EPnP  and  SDR 
pose  esti  mators.  Th  i  s  f  i  gu  re  d  i  spl  ays  the  pose  esti  mati  on  accu  racy  for  a  f  i  xed  6-poi  nt  u  ncentred 
conf  i  gu  rati  on .  N  otethat  i  n  thi  s  and  al  I  remai  ni  ng  f  i  gures  of  thi  s  paper,  w  e  no  I  onger  d  i  sti  ngu  i  sh 
between  SDR-fCG  and  SDR  and  assume  that  local  fine-tuning  forms  part  of  theSDR  algorithm. 
FromFigure2theLFIM  performance  accu  racy  is  clearly  poor  even  for  pixel  errors  as  smal  I  as  2. 
The  I  arge  devi  ati  on  of  the  RM  SE  error  i  s  dueto  osci  I  lati  ons  between  two  possi  bl  e  poses,  one  of 
which  presenting  a  lower  minimum.  Flistograms  given  in  Figure  3  illustrate  this  point  for  a 


1MLFIM:  http:/  /  www.emt.tuqraz.at/  -pinz/  code 
EPnP:  http:/  /  cvlab.epfl.ch/  software  EPnP/ 
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pixel  measurement  error  of  5.  As  many  as  550  simulation  trials  (out  of  3000)  show  the  LH  M 
algorithm  converging  to  thewrong  pose.  N  ote  also  how  the  EPnP  estimation  error  lies  within  a 
much  larger  interval  compared  to  that  of  the  SDR  algorithm. 


RMS  Position  Error 


Gaussian  Image  Noise  (Pixels) 


RMS  Rotation  Error 


Gaussian  Image  Noise  (Pixels) 


Median  Position  Error 


Gaussian  Image  Noise  (Pixels) 


Median  Rotation  Error 


Gaussian  Image  Noise  (Pixels) 


F igu re  1:  RM  S  and M  edian  E sti m ati on  E rrors  ( P osi ti on  and R  otati on)  using  6  uniformly  di stri bu ted 
reference  points 


RMS  Position  Error 


RMS  Rotation  Error 


Gaussian  Image  Noise  (Pixels) 
Median  Position  Error 


Gaussian  Image  Noise  (Pixels) 
Median  Rotation  Error 


Gaussian  Image  Noise  (Pixels) 

Figure2:  RMS  and  M  edian  Estimation  Errors  (Position  and  Rotation)  using6  fixed  reference  points 
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LHM 


Rotation  Error  (deg) 


Rotation  Error  (deg) 


SDR 

E 

0  1  2  3  4  5 


Rotation  Error  (deg) 


Figure3:  R  otation  error  histograms  for  a  pixel  error  of  5  using  3000  realisations. 


4.2  Pose  Errors  versus  N  umber  of  Reference  Poi nts 

The  next  plot  in  Figure4displaysthepose(RMSE)  asafuncti  on  of  number  of  reference  poi  nts. 
The  pi  xel  measurement  error  i  s  mai  ntai  ned  at  5  and  the  references  poi  nts  aresel  ected  randoml  y 
within  thecube  [-1,  l]x[-l,  l]x[5,  7] .  As  illustrated  in  Figure4  it  is  expected  to  observethat 
the  pose  estimation  accuracy  improves  for  all  algorithms  as  the  number  of  reference  points 
increases.  What  is  less  obvious  is  the  considerable  error  increase  for  low  point  counts, 
particularly  when  n  =4  .This  behaviour  could  be  explained  by  the  fact  that  the  4point  pose 
problem  may  often  be  considered  as  a  perturbation  of  a  3-point  poseproblem  (Flaralick  el  al. 
1991),  which  is  known  to  haveas  many  as4sol utions,  or  equivalently  4 global  minima.  Adding 
thefourth  poi  nt  often  i  ntroduces  a  smal  I  di  sturbanceto  these  mi  ni  ma,  maki  ng  it  hard  for  pose 
search  algorithms  to  distinguish  them.  Despite  this,  Figure  4  clearly  shows  the  superior 
performance  of  the  SDR  algorithm  at  coping  with  local  minima.  This  is  to  be  expected,  as  the 
SDR  formulation  (13)  is,  by  design,  a  close  convex  approximation  of  the  original  nonconvex 
problem  (10). 


RMS  Position  Error  RMS  Rotation  Error 


Figure  4:  RMS  andM  edian  estimation  errors  (position  and  rotation)  versus  number  ofreferencepoints 
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4.3  Pose  Accuracy  Associated  With  Coplanar  Reference  Points 

Figure  5  depicts  the  pose  accuracy  for  a  coplanar  point  configuration.  We  assume  a  scenario 
where  10  points  lie  on  a  plane  pitched  by  45°  and  the  objective  is  to  assess  the  performance 
accuracy  asafunction  of  pixel  error.  All  10  points  are  located  inthebox  [-1, 1]  x  [— 1,  l]x[6,  6] . 
Thetwo  pi  ots  on  the  I  eft  of  Fi  gure  5  depi  ct  the  posi  ti  on  and  rotati  on  accuracy  of  the  M  L  FI  M  and 
SDR  algorithms  when  all  outliers  are  removed  (i.e.,  RMS  errors  are  computed  after  removing 
outl  i  ers).  Thethi  rd  pi  ot  on  the  ri  ght  show  the  percentage  of  outl  i  ers  for  M  L  FI  M ,  SD  R  as  wel  I  as 
L  FI  M .  A  s  i  s  cl  ear  i  n  thefi  gure,  both  M  L  FI  M  and  SD  R  exhi  bit  si  mi  I  ar  behavi  ours  i  n  terms  of  pose 
accuracy  and  percentage  of  outliers.  The  L  FI  M  algorithm  performs  poorly  even  in  the  absence  of 
pixel  measurement  errors. 


RMS  Position  Error  RMS  Rotation  Error  Outliers  Percentage 


Figure5:  Pose  accuracy  associated  with  a  coplanar  point  configuration  tilted  by  45°.  Left:  position 
error.  M  iddle:  rotation  error.  Right:  Percentage  of  pose  outliers  as  a  function  of  pixel  error. 

4.4  Runtime  Comparison 

The  M  atl  ab  generated  pi  ots  i  n  Fi  gure  6  present  the  average  convergence  runti  me  as  a  fundi  on 
of  number  of  poi  nts  for  L  FI  M  and  SD  R.  The  number  of  poi  nts  vari  es  from  4 to  1000.  For  a  smal  I 
number  of  points  the  Ol  algorithm  is  obviously  faster  to  run.  The  EPnP  algorithm,  whose 
runti  me  is  not  shown  here,  isthefastest(Lepetitetal.  2008).  FI  owever,  oneshould  notlosesight 
of  the  fad  that  other  equal  I  y  i  important  performance  i  nd  i  cators  exi  st  and  need  to  be  consi  d  ered . 


Runtime  comparisons 


Figure  6:  A  Igorithm  runtime  comparison  plot 
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I  n  w hat  fol  I  ows  we  provi  de  a  bri  ef  criti  cal  summary  regardi  ng  the  overal  I  performance  of  the 
LHM,  EPnP  and  SDR  pose  algorithms,  based  on  numerous  computer  simulations  and 
observations  made  in  (Lepetitetal.  2008).  The  LH  M  algorithm  suffers  from  convergence  to  local 
minima  unless  it  is  properly  initialised.  If  it  converges  to  the  global  minimum,  then  its 
performance  accuracy  is  bdter  than  that  of  EPnP.  The  EPnP  algorithm  does  not  need 
initialisation,  is  lesslikely  to  con  verge  to  a  w  rang  pose,  parti  cu  I  arl  y  for  smal  I  er  pi  xel  errors,  but 
its  performance  accuracy  tends  to  be  lower  than  that  of  the  LH  M  algorithm.  Both  LHM  and 
EPnP  algorithms  are  prone  to  perform  poorly  when  presented  with  tilted  planar  point  scenes. 
The  SDR  algorithm  displays  much  higher  performance  accuracy  than  EPnP  and  is  the  least 
vul  nerabl  eto  converge  to  a  I  ocal  mi  ni  mum.  The  SD  R  sol  uti  on  can  al  ways  be  verifi  ed  for  gl  obal 
optimality  (by  checking  the  associated  SDP  solution  rank),  a  feature  which  is  not  avail  able  for 
the  other  two  approaches.  Its  runtime  may  be  perceived  asslowforsomeapplications,  but  only 
for  I  ow  poi  nt  count  scenari  os  (under  50).  A  s  the  number  of  poi  nts  is  i  ncreased  the  SD  R  method 
becomes  significantly  faster  than  the  LH  M  iterative  algorithm.  To  be  useful  for  near-realtime 
applications,  further  speedup  is  recommended.  This  can  be  achieved  by  optimising  the 
algorithm's  codeand  writing  an  efficient  interior  point  method  (Boyd  and  Vandenberghe  2004) 
al  gori  thm  that  sol  ves  ( 13) .  O  ne  shou  I  d  stress  that  an  average  ru  nti  me  of  50  ms  for  a  poi  nt  cou  nt 
of  100  represents  a  significant  speedup  improvement  among  algorithms  that  rigorously 
implement  global  search  for  the  optimal  pose  (compare  this  time  with  the  average  runtime  of 
morethan  a  minute  using  the  branch-and-bound  search  technique  of  (Hartley  and  Kahl  2009). 

4.5  Pose  Estimation  Using  a  Real  Image 

Figure  7  shows  a  real  image  of  a  hexagonal  box  taken  by  a  calibrated  camera  having  a  focal 
I  ength  of  756  pi  xel  s  and  w  hose  i  mage  resol  uti  on  i  s  640  x  480 .  A  s  many  as  10  corners  bel  ongi  ng 
to  the  box  were  located  in  the  image  and  used  in  the  camera  pose  estimation  process  by 
applying  the  SDR  algorithm  developed  in  this  work.  The  estimated  camera  orientation  with 
respect  to  the  box  axes,  shown  i  n  bl  ack  i  n  the  i  mage,  i  s  found  to  be  (rol  I  =31.71°,  pitch= 64.42°, 
yaw— 12.28°).  The  estimated  camera  position  with  respect  to  the  box  centre  is  computed  as 
[~41.51,40.8,-23.76]r  cm  (which  corresponds  to  a  translation  vector  of 
t=[— 0. 1 8, 2.34, 62.84] 7  cm).  To  visualise  the  pose  estimation  accuracy,  the  estimated  camera 
pose  parameters  are  then  used  to  re-projecttheboxwire-frame  model  on  the  image  as  shown  in 
Figure  7. 
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Figure  7: 


Model  reprojection  According  To  The  Estimated  Pose 


100  200  300  400  500  600 

The  reprojection  of  the  box  model  on  theoriginal  image  according  to  the  pose  estimated  using 
our  SDR  algorithm 


5.  Conclusions 

In  this  paper  we  have  presented  an  efficient  non-iterative  pose  estimation  approach  that 
globally  solves  for  the  camera  position  and  orientation.Theapproach  relies  on  SDP  relaxation  of 
a  quartic  object  space  error,  expressed  in  terms  of  quaternion  element  products.  A 
comprehensive  comparative  performance  analysis,  carried  out  in  the  computer  simulations 
section,  highlights  the  superior  performance  of  the  SDP  relaxation  based  solving  method.  Its 
main  features  are:  (1)  O(n)  computational  complexity,  (2)  improved  estimation  accuracy  for 
both  coplanar  and  non-coplanar  configuration  points  and  (3)  relatively  high  convergence  speed, 
which  is  reasonable  for  many  time  non-criti  cal  applications. 
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AppendixA:  Proof  of  Equivalence 


To  prove  that  (1)  is  equivalent  to  (2),  we  make  use  of  the  cross  product  property, 

|<2  x6||2  =  ||a||2 ||6||2  ~{aTb\  ,  which  leads  to 


£(R,t)  =  Y4wi  ||Rp,  +  tf  — (vf (Rp;  + 1))2 

tf  L  v/ v/  J 

=  X w-  *RP  +  t)r(Rp,  + 1) - tM(rP,  +  tf- V,.)(vf (Rp,  + 1)) 


By  factoring  out  Rp  ,  + 1 ,  this  expression  reduces  to 


£(R,t)  =  |>;.(Rp,.+t)7'  I 


f  T  \  (  T 

V  V  V .  V  . 

satisfies  I — =  I — ‘—L- 

{  V,  vj  l  V,.V,. 


(Rp,+t).  Since  the  projection  matrix  I-- 


V  v 

I — ,  then  (2)  follows  as  a  result, 
v.  V 

v  i  i 
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Appendix  B:  System  M  atrices  and  Linear  Constraints 


z  = 


x 

0 


0 

Y 


14x14 


where 


<h 

q\q2 

q  i  q2 

q  \  q  4 

q[q  2 

q\q2 

2  2 

q  i  q  2 

q[q2q2 

qf  q  2q  4 

q  2q  i 

qUi 

qx  q2q3 

2  2 

q  i  q  3 

q\q2q  4 

qlq^i 

q  i  q  4 

q\q2q  4 

q\q2q  4 

2  2 

qlqtf4 

2  2 
qxq2 

q  2q  i 

q2q^q2 

q  2q  \  q  4 

q  2 

q{  q^q-*, 

q2q\q2 

q2q{q2 

q{q2q3q4 

q  2q  3 

qf  q2q  4 

qlqtf4 

qtq2q3q4 

qUrti 

q\q  4 

2  2 

q  \  q  i 

qU\q2 

q^l 

<?3<M4 

2  2 
q2<h 

q\  q^q  4 

qtq2q3q4 

q\q \q  4 

qlq^i 

q;q3q4 

2  2 

qlqxq2 

qlqxq2 

qUi 

2  2 
q2<i4 

q;q2q2 

qlq2q4 

qiq2 

q;q2q4 

q;q~4 

q2qiq2 

qn2qtq4 

q  3  q iq  2 

q{q2q3q4 

qlqxq2 

qlq{q2 

qtq2q3q4 

q2q\ 

q\ q^4 

q;qtq3 

qlq2qiq4 

qU\q2 

qlqxq4 

qiq^i 

qUi 

q  2q  3 

q  2q  4 

2  2 

q2q3 

q  2q  3q  4 

2  2 

q  2q  4 

2  2 

q2q3 

q2q3q4 

q2q2 

q  3  q  2q  4 

q  4q2q2 

q\q2q4 

2  2 

q2<i4 

qlq2q4 

qlq2q2 

q\q2 

q\q2 

q\q2q  4 

q  3 

q\q  4 

2  2 

q2q  4 

q]q2q4 

q]q2q2 

q\q  4 

2  2 
q3q* 

q\q2 

qlq2q2 

q\q2 

2  2 

<73<?4 

q\q2 

q  4 

and 


2 

q\ 

q\q2 

q\q2 

qxq4 

^2 

g3 

e4 

Y  =  qqr  = 

q\q2 

2 

q  2 

q2q2 

2 

q2q  4 

— 

<?5 

g6 

e7 

q\q2 

q2q2 

q2 

q2q4 

e3 

e6 

g8 

eg 

qxqA 

q2q4 

q2q  4 

ql  _ 

_e4 

el 

e9 

eio_ 
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A  dose  examination  of  the  upper  triangular  region  of  X  reveals  20  equal  entry  pairs.  These 
transl  ate  i  nto  the  20 1  i  near  rel  ati  onsh  i  ps, 


x 

x 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 

X 


4,4 

7.7 

9.9 
2,2 
6,6 

3.3 

4.7 

7.9 

9.4 

2.3 

3.5 

3.6 

3.7 

7.8 

4.8 
6,7 

4.5 

3.4 

2.4 

4.6 


-Xuo=0 

-x5llo  =  o 
_X8,10  =  0 
-x1>5=0 
-X5,8=0 
-xli8  =  0 

_X2,io  =  0 

-x6>10  =  o 

_X3,10  =  0 

-Xi.6=0 

-X2.6=0 

-X2,8=0 

-x2i9=o 

-X6.9=0 

-X39  =0 
-X5,9=0 
-X27  =0 


-X|,9=0 
—  X|,7=0 

-x2j9=o 


Using  the  property  q.qj  =  q^jiqf  +  ql  +  q]  +  q] ) ,  the  following  10  linear  constraints  are 
established 


Xy 

+  X2,2 

+  X3,3 

+  X4,4~ 

Yy  = 

-  0 

X2,2 

:  +  X5,5 

+  X6,6 

i  +  X7,7~ 

-y2,2 

=  0 

X3,3 

+  X6.6 

+  X8,8 

+  X9,9~ 

=  0 

x4,4 

+  X7,7 

'  +  X9,c 

I  +  X10,1C 

II 

I 

X2,, 

+  X2,5 

+  X2,8 

+  X2,10' 

-Yu 

=  0 

X3,l 

+  X3.5 

+  X3,8 

+  X3,10“ 

'Yy  : 

=  0 

X4,1 

+  X4,5 

+  X4,8 

+  X4,10' 

-ym 

=  0 

X6,l 

+  X6.5 

+  X6.8 

+  X6,10' 

-Yy 

=  0 

X7,l 

+  X7,5 

+  X7,8 

+  X7,10' 

-Y2,4 

=  0 

X9,l 

+  X9,5 

+  X9,8 

+  X9,10_ 

-  Y3,4 

=  0 

The  normalisation  constraint  q]+q22+q]+q24=  1  can  be  expressed  as 

Yi>1  +  Y2>2  +  Y3j3  +  Y4j4=1. 
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AppendixC:  Implementation  of  Sedumi  Algorithm 


z  =  sedumi{ A, b,c) solves  minimise  crz  s.t.  Arz  =  b  &z>0 


where  cr  =  [[M(l :  10,1)  •••  M(1 : 10,10)1  zeros(l,16),  100 -a]  (1x118)  (notea  =0.002) 
b  =  [zeros(l,30),  1,  (32x1)  (note  s  =0.3) 

z  =  HX(1 : 10,1)  •••  X(1 : 10,10)1  [Y(l :  4,1)  ■••¥(! :  4,4)1  *017),  z(118)f  (118x1) 
and  A  is  given  as 


A  = 


K,  (1:10,1) 

K31(l :  10,1) 

0 1 0x1 

K,  (1:10,2) 

K31  (1:10,2) 

0 1 0x1 

Kj  (1:10,10) 

531  (1:10,10) 

0 1 0x1 

5,(11:14,11)  ••• 

K31(l  1:14,11) 

5(11:14,11) 

5,(1  1:14,12) 

K31(l  1:14,12) 

5(11:14,12) 

5,(11:14,13)  ••• 

K31(l  1:14,13) 

5(11:14,13) 

iCI(ll:14,14) 

K31(l  1:14,14) 

5(11:14,14) 

0  ®lx29 

-1 

0 

®  ®lx29 

0 

-1 

118x32 


In  Matlab,  theA  matrix  is  constructed  as  shown  below. 

%%%%%%%%%%%%%%%%%%%%%%%%%%0/o%%%%%0/o%%%0/o0/o0/o%%%%%%%% 


A=sparse (zeros ([118,32])); 


%  constraints  1-20: 


A  (  9 1 , 1 )  =  1 ; 

A (34 , 1) = - 1 ; 

o, 

o 

constraint 

1 

X(l,10) -X (4,4) =0 

A  ( 9  5 , 2 ) =1; 

A(67,2) = - 1 ; 

O, 

"O 

constraint 

2 

X  ( 5 , 10 ) -X ( 7 , 7) =0 

A  (  9  8 , 3  )  =  1 ; 

A(89,3)=-l; 

Q 

"O 

constraint 

3 

X ( 8 , 10 ) -X ( 9 , 9 ) =0 

A(41,4) =1; 

A(12,4) =-l; 

O, 

"O 

constraint 

4 

X ( 1 , 5 ) -X ( 2 , 2 ) =0 

A  (  7  5 , 5 )  =  1 ; 

A (56, 5) =-l; 

a 

o 

constraint 

5 

X ( 5 , 8 ) -X ( 6 , 6 ) =0 

A  ( 8 , 6 )  =  1 ; 

A(23,6)=-l; 

o, 

"o 

constraint 

6 

X ( 8 , 1 ) -X ( 3 , 3 ) =0 

A ( 64 , 7 ) =1; 

A  ( 92 , 7 )  =  - 1  ; 

a 

o 

constraint 

7 

X (4 , 7 ) -X(2, 10) =0 

A (87, 8) =1; 

A  ( 9  6 , 8 )  =  - 1  ; 

Q 

"o 

constraint 

8 

X ( 7 , 9 ) -X(6, 10) =0 

A ( 3  9 , 9 ) =1; 

A  ( 93 , 9 )  =  - 1  ; 

o, 

"o 

constraint 

9 

X ( 9 , 4 ) -X ( 3 , 10 ) =0 

A ( 2  2 , 10) = 1 ; 

A(51, 10)  =  - 1 ; 

a 

"o 

constraint 

10 

X ( 2 , 3 ) -X ( 1 , 6 ) =0 

A (52, 11) =1; 

A(25, 11) =-l ; 

o, 

o 

constraint 

11 

X ( 2 , 6 ) -X ( 5 , 3 ) =0 

A (53, 12) =1; 

A(18, 12) =-l; 

a 

o 

constraint 

12 

X ( 3 , 6 ) -X ( 8 , 2 ) =0 

A(82, 13) =1; 

A(63, 13) =-l; 

a 

o 

constraint 

13 

X ( 2 , 9 ) -X(3, 7) =0 

A(86, 14) =1; 

A(68, 14) =-l; 

o, 

"o 

constraint 

14 

X ( 6 , 9 ) -X(8, 7) =0 

A(83, 15) =1; 

A(38, 15) =-l; 

o, 

"o 

constraint 

15 

X ( 3 , 9 ) -X ( 8 , 4 ) =0 

A(66, 16) =1; 

A(85, 16) =-l; 

o, 

"o 

constraint 

16 

X ( 6 , 7 ) -X ( 5 , 9 ) =0 

A(62, 17) =1; 

A(35, 17) =-l; 

a 

o 

constraint 

17 

X ( 2 , 7 ) -X ( 5 , 4 ) =0 

A(33, 18) =1; 

A(81, 18) =-l; 

a 

o 

constraint 

18 

X ( 3 , 4 ) -X ( 1 , 9 ) =0 

A(32, 19) =1; 

A(61, 19) = - 1 ; 

a 

o 

constraint 

19 

X ( 2 , 4 ) -X ( 1 , 7 ) =0 

A  ( 8  2 , 2  0 )  =  1 ; 

A(54, 20) =-l; 

o, 

"o 

constraint 

20 

X ( 2 , 9 ) -X (4,6) =0 
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a 

"o 

constraint 

21 

o, 

o 

constraint 

22 

o, 

o 

constraint 

23 

o. 

o 

constraint 

24 

o 

o 

constraint 

25 

o. 

o 

constraint 

26 

o 

o 

constraint 

27 

Q 

o 

constraint 

28 

o 

o 

constraint 

29 

o 

o 

constraint 

30 

o, 

o 

constraint 

31 

X ( 1 , 1 ) +X ( 2 , 2 ) +X (3,3) +X (4,4) - Y ( 1 , 1 ) =  0 
X ( 2 , 2 ) +X(5,5) +X (6,6) +X (7,7)  - Y ( 2 , 2 ) =  0 
X ( 3 , 3 ) +X ( 6 , 6 ) +X (8,8) +X (9,9)  - Y ( 3 , 3 ) =  0 
X ( 4 , 4 ) +X ( 7 , 7 ) +X (9,9) +X (10,10) - Y ( 4 , 4 ) =  0 
X (2 , 1 ) +X (2,5) +X (2,8) +X (2,10) -Y (1,2) =0 
X ( 2 , 1 ) +X (3,5) +X (3,8) +X (3,10) -Y ( 1 , 3 ) =0 
X (4 , 1 ) +X (4,5) +X (4,8) +X (4 , 10 ) -Y (1,4) =0 
X ( 6 , 1 ) +X (6,5) +X (6,8) +X (6,10) -Y (2 , 3 ) =0 
X ( 7 , 1 ) +X (7,5) +X (7,8) +X (7,10) -Y (2,4) =0 
X ( 9 , 1 ) +X (9,5) +X (9,8) +X (9,10) -Y (3,4) =0 
Y(l,l)+Y(2,2) +Y (3,3) +Y (4,4) -z (117) =12 


A ( 1 , 2 1 ) =1; 

A  ( 1 2 , 2  2  )  =  1 ; 
A ( 2  3 , 2  3 ) =1; 
A ( 3  4 , 2  4 ) =1; 
A  ( 2 , 2  5 )  =  1 ; 

A ( 3 , 2  6 ) =1; 

A (4 , 2  7 ) =1; 

A (6, 28) =1; 

A ( 7 , 2  9 ) =1; 

A  ( 9 , 3  0 )  =  1  ; 
A(101, 31) =1 


A  ( 12 , 21 ) =1; 

A(23,21)=l; 

A  ( 34 , 2 1 ) =1; 

A(101, 21) = 

-l; 

o, 

o 

const 

21 

A(45,22) =1; 

A(56,22)=l; 

A(67,22)=l; 

A (106 , 22 ) = 

-l; 

o, 

o 

const 

22 

A(56,23) =1; 

A(78,23)=l; 

A  ( 8  9 , 2  3  )  =  1 ; 

A  (111 , 23 )  = 

-l; 

o, 

o 

const 

23 

A(67,24) =  1; 

A(89,24)=l; 

A ( 100 , 24 ) =1; 

A  ( 116 , 24 )  = 

-l; 

o, 

o 

const 

24 

A(42, 25) =1; 

A(72,25)=l; 

A ( 92 , 25 ) =1; 

A (105 , 25) = 

-l; 

o, 

~o 

const 

25 

A(43, 26) =1; 

A(73,26)=l; 

A (93, 26) =1; 

A ( 10  9 , 2  6 )  = 

-l; 

o 

~o 

const 

26 

A (44 , 27) =1; 

A(74,27)=l; 

A (94,27) =1; 

A (113 ,21) = 

-l; 

o 

o 

const 

27 

A(46, 28) =1; 

A(76,28)=l; 

A  ( 9  6 , 2  8 )  =  1 ; 

A(110, 28) = 

-l; 

a 

o 

const 

28 

A(47, 29) =1; 

A(77,29)=l; 

A  (  97 , 2  9 ) =1; 

A ( 114 , 2  9 )  = 

-l; 

o, 

o 

const 

29 

A(49,30)=l; 

A(79,30)=l; 

A(99,30)=l; 

A(115, 30) = 

-l; 

o, 

"o 

const 

30 

A ( 106 , 3 1 ) =1 

; A ( 1 1 1 , 3 1 ) =1 

; A ( 1 1 6 , 3 1 ) =1; 

A(117, 31) = 

-l; 

a 

o 

const 

31 

%constraint  32 

%h  (1)  Y  ( 1 , 1 )  +h  (2 )  Y  ( 1 , 2  )  +h  (3  )  Y  (1 , 3  )  +h  (4 )  Y  ( 1 , 4 )  +h  (5)  Y  (2 , 2 )  +h  (6 )  Y  (2 , 3  )  +  ... 
%h (7 ) Y (2 , 4 ) +h ( 8 ) Y (3 , 3 ) +h (9 ) *Y (3 , 4 ) +h ( 10 ) *Y (4 , 4 )  -z  ( 118 ) =0 . 3 

A ( 101 , 32 ) =  H ( 1 ) ;  A(105,32)=  H(2);  A(109,32)=  H ( 3 ) ; 

A ( 113 , 32 ) =  H ( 4 ) ;  A(106,32)=  H(5);  A(110,32)=  H(6); 

A  ( 114 , 32 )  =  H ( 7 ) ;  A(lll,32)=  H ( 8 ) ;  A(115,32)=  H(9); 

A ( 116 , 32 ) =  H ( 1 0 ) ;  A (118,32)=-!; 


The  optimal  rotation  can  be  computed  by  firstly  extracting  Y  matrix  from  the  solution  z  and 
then  constructing  the  rotation  matrix  R. 


%extract  Y  matrix  from  z 
Y=mat (z (101 : 116)  ,4) ; 


%Construct  the  rotation  matrix  R 

R= [ Y  (1,1) +Y (2, 2) -Y (3, 3) -Y (4, 4) ,2*(Y(2,3) +Y (1,4) ) ,2* (Y (2,4) -Y (1,3) ) ; . . . 
2*(Y(2,3)-Y(1,4)  )  ,Y(1,1)-Y  (2,2)+Y(3,3)-Y  (4,4)  , 2* (Y (3 , 4 ) +Y  (1,2)  )  ;  .  .  . 

2* ( Y (2 , 4 ) +Y ( 1 , 3 ) ) , 2* ( Y (3 , 4 ) -Y ( 1 , 2 ) ) , Y ( 1 , 1 ) -Y (2 , 2 ) -Y ( 3, 3) +Y ( 4 , 4 ) ] ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


The  optimal  translationtcan  be  obtained  by  inserting  R  into  Equation  (5)  in  Section  2,  and  then 
the  camera  position  is  computed  as  pcamera  =  -Rrt . 

The  graphical  representation  of  the  sparse  A  matrices  for  SDR  and  SOSP  are  shown  below  for 
dimensional  comparisons. 


2  Constrai  nt  31  is  equivalent  to  Y(l,l)+Y (2,2)+Y (3,3)+Y (4,4)  >  1,  which  speeds  up  the  SeDuM  i  runti  me, 
and  Y(l,l)+Y(2,2)+Y(3,3)+Y(4,4)  will  be  brought  very  close  to  1  at  the  end  of  the  run. 
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Figure8:  I  mage  representation  of  A  matrices  used  as  input  to  SeD  uM  i  Software 
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Appendix  D :  Sum  of  Squares  Programming 


A  multi  variate  polynomial,  p(x ) ,  of  degree  2/ ,  is  a  sum  of  squares  (SOS)  if  it  can  be  expressed 
as  a  finite  sum  of  polynomial  squares  (i.e.  p(x)  =  ^p;(x) ).  This  clearly  means  that  any  SOS 

polynomial  can  bewrittenas  p(x)  =mr(x)Qm(x)  .where Q  isasemidefinitepositivematrix 
consisting  of  affine  entries  and  m(x)  is  a  monomial  vector  whose  largest  degree  is  / .  SOS 
polynomials  are  necessarily  non-negative,  but  not  all  non-negative  polynomials  are  SOS. 

A  global  minimiser  of  the  multivariate  polynomial,  F(x),  isobtained  by  solving 


minimise  F(x ) 

xeRm 

which  is  equivalent  to  solving 

maximise  y 
subject  to  F{x)  -y>  0 

A  lower  bound,  y,  to  (D2)  can  be  obtained  by  relaxing  (D2)  as 


(Dl) 


(D2) 


maximise  y 

(D3) 

subject  to  F(x)  -  y  is  SOS 

whose  solution  is  computable  in  polynomial  time  using  semi  definite  programming. 

Now  letusassumewearegiven  additional  equality  and  inequality  constraints,  leading  to  the 
constrained  minimisation  problem 

minimise  F(x) 

subject  to  g,  (x)>0,  i  =  (D4) 

hj(x)  =  0,  j  =  \,---,N 

The  lower  bound,  y ,  for  (D4)  can  be  found  using  the  Positivstellensatz-based  relaxations 

F(x)  -y  =  <7 0 ( x )  +  £ Xj (. x)hj (x)  +  £  v,(x)gl(x)  +  'Z<7n,i2(x)gn(x)gn(x)  +  ---  (D5) 

j  i  il,i2 

where  a:(x)  are  SOS  polynomials  and  Xj(x)  are  polynomials.  Similar  to  the  unconstrained 
problem  (D3),  maximising  y ,  subject  to  SOS  relaxation  of  (D5)  can  be  computed  using  a 
semidefinite  program  for  which  efficient  well-established  algorithms  (e.g„  interior  point 
methods)  exist.  The  lower  bound,  y ,  approaches  the  global  minimum  of  (D4)  asthedegreeof 
(D5)  i s  i ncreased .  I  n  practi ce  a  moderate  i  ncrease  i s  often  enough  to  result  i  n  a  hi ghly  accurate 
approxi  mati  on  of  the  gl  obal  mi  ni  mum.  When  this  occurs,  the  SOS  relaxati  on  i  s  sai  d  to  be  ti  ght. 
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Pose  Estimation  Using  Sum  of  Squares  Programming  (SOSP) 

In  this  section  we  offer  a  unified  approach  to  solve  the  pose  estimation  problem  using  SOS 
rel  axati on  i  rrespecti  ve of  w hether  the  reference  poi  nts  are copl  anar  or  non-copl  anar .  The  w ork 
in  (Schweighofer  and  Pinz  2008)  makes  this  distinction  and  formulates  a  number  of  SOS 
rel  axation  al  gorithms  to  account  for  thetype  of  reference  poi  nts.  I  n  what  fol  I  ows  we  provi  de  a 
detailed  SOS  relaxation  applicableto  thegeneral  case  based  on  solving 

minimise  er(q)Me(q) 
q 

subj  ect  to  h  T  e(q)  >  0  ( p  gj 

kre(q)  =  1 
qx>0 

which  is  based  on  (10).  This  new  formulation  emphasises  that  the  unknown  vector  is 
q  =  [ql,q1,q'i,qi\T  and  addsanother  inequality  constraint,  >0  ,  w  hi  ch  serves  to  d  i  sti  ngui  sh 
equally  valued  global  minima  generated  by  opposite  solutions  q  and  -q.  The  objective 
function  is  F(q)  =  er(q)Me(q)  which  is  a  4-variable quartic  polynomial. 

If  weputg^q)  =  hre(q) ,  g2(q)  =  a  and  h( q)  =  kre(q)  - 1 ,  then  usingthePositivstellensatzand 
SOS  decomposition,  a  4-degree  SOS  relaxation  problem  may  be  formulated  as 

maximise  y 

(D7) 

subject  to  C(q)  is  SOS 

where  the  expression  of  C(q)  being  given  by 

C(q)  =  F(q)  -  y  -  A(q)/t(q)  -  <x,  (q)g,  (q)  -  rx2  (q)g2  (q)  -  cr12  (q)g,  (q)g2  (q) 
with  cTj ,  cr2 ,  cr12  being  SOS 

The  expressions  for  the  introduced  parameter  polynomials  are 

^(q)  =  [l;q]rQ,[l;q] 

^2(q)  =  [l;qfQ2[l;q] 

cr12(q)  =  a  and 
A(q)  =  [l;q]rQ0[l;q] 

with  Q[  >0,  Q2>0  and  a  a  non-negative  scalar.  The  degrees  of  the  SOS 
polynomials ax ,  <j2,  <jX2  and  the  polynomial,  A ,  are  determined  by  the  requirement  that  the 
degreeof  C(q)  is4and  the  property  that  SOS  polynomials  areeven  degree  polynomials.  The 
matrix  Q0  is  selected  to  bean  upper-triangular  matrix  with  15  unknown  coefficients. 
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TheSOS  relaxation  of  (D7)  means  that  C(q)  can  also  bewritten  as  C(q)  =  mr(q)Qm(q)  where 
Q  >  0  and  m(q)  =  [l;q;e(q)]  is  a  15-dimensional  vector  whose  largest  degree  is  2.  By 
expanding  and  matching  both  expressions  of  C(q) ,  all  coefficient  matrix  entries  of  Q0 ,  Q, ,  Q  2 , 
Q  and  the  seal  ars  a ,  y  could  berelated  affinely.  With  theaid  of  a  SO  SP  software  package  such 
as  SOSTOOLS  (Prajna  et  al.  2004),  the  matching  process  can  be  automated  and  the  SDP 
formulation  may  be  established.  However,  duetothelargedimensions  of  thesparse  matrix  A 
(size:  292  x  70 )  and  vectors  b  and  c  ,  these  are  not  i  nd  uded  i  n  the  paper  and  are  obtai  nabl  e 
from  the  authors.  N  ote that  i  f  one  w  i  shes  to  f u  rther  ti  ghten  the  SO  S  rel  axati  on  by  i  ncreasi  ng  the 
degree  beyond  4,  then  the  matrix  A  becomes  inconveniently  large,  resulting  in  a  significant 
i  ncrease  i n  processi  ng  requi  rement. 
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